/*
 * TestGoToPoint.java
 *
 * Created on 22 de agosto de 2007, 11:39 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Test;
import Behaviors.Arbitrators.*;
import Behaviors.Schemas.*;
import Behaviors.Schemas.Motor.*;
import Behaviors.Schemas.Perception.*;

import Robot.*;
import Robot.Sensors.*;

import Utils.*;
/**
 *
 * @author jonathan
 */
public class TestGoToPoint {

    /**
     * Creates a new instance of TestGoToPoint
     */

    
    public static void process() {
        
        
       
    //***Definicion Robot 1*************************************
    /*Robot robot1=new Robot("127.1.1.1", 6665,"Robot 1",0);
    try{
    RealPosition2d position1=new RealPosition2d(robot1,"position1",0);
    }
    catch(Exception e){
        e.printStackTrace();
    }
    */
    //***Definicion Robot 3*************************************
    Robot marlin=new Robot("Marlin","192.168.1.1",6665, 0);
    
    //Robot camara=new Robot("127.0.0.1",6665,"Camara",0);
    
      //*********************************************************
   /* RealPosition2d positionCam;
    try{
        positionCam=new RealPosition2d(camara,"positionCamara",0);
    }
    catch(Exception e){
        e.printStackTrace();
        return;
    }
   */
    
    RealPosition2d positionMarlin;
    try{
        positionMarlin = new RealPosition2d(marlin,"marlinPos",0);
    
    }
    catch(Exception e){
        e.printStackTrace();
        return;
    }
    
    IR ir;
    try{
        ir= new IR(marlin,"marlinIR",0);
    
    }
    catch(Exception e){
        e.printStackTrace();
        return;
    }
    
   // 
    /*FusionPosition2d positionMarlin;
    try{
        positionMarlin=new FusionPosition2d(camara,marlin,"positionMarlin",0);
    }
    catch(Exception e){
        e.printStackTrace();
        return;
    }*/
    marlin.runThreaded(10,0);
    
    while(!ir.updatePoses());
    //camara.runThreaded(10,0);
        //Formacion formacion=new Formacion();
     

    //positionCenter.setPosition(new Pose2d(0,0,(float)Math.toRadians(90.0f)));
        //positionCenter.setPosition(new Pose2d(-6.5f,-6.5f,0.79f));
    
    Vector2d point=new Vector2d(100,100,Vector2d.MODE_XY);
        
    System.out.println("PointAngle="+Float.toString((float)point.getAngle()));
        
    NxMSGoToPoint goto2=new NxMSGoToPoint("goto2",positionMarlin,point,NxMSGoToPoint.MODE_EXPONENTIAL);
    NxPSDetectObstaclesIR obst = new NxPSDetectObstaclesIR("goto2",positionMarlin,ir);
    NxMSAvoidObstacles avoid = new NxMSAvoidObstacles("goto2",obst,positionMarlin,NxMSAvoidObstacles.MODE_EXPONENTIAL);
    //goto2.setMaxVel(0.3f);
    goto2.setGain(1.0f);
        //DetectFormation detectFormation=new DetectFormation(position1, positionCenter);
        //MaintainFormation mantainFormation=new MaintainFormation(detectFormation);
    
    Behavior[] array = {goto2,avoid};
    CooperativeArbitrator arbitrator1=new CooperativeArbitrator("a1", array,positionMarlin, false);
    arbitrator1.setDebugMode(true);   

    while(true)
    {
        arbitrator1.step();
        try
        {
        Thread.sleep(50);
        }
        catch(Exception e){}
    }
        
       /* while(true)
        {
            positionCenter.setSpeed(0.0f,0.5f);
            //controller.run();
            try{
                Thread.sleep(100);
            }
            catch(Exception e){}
            
        }*/
        
        
    }
    

}
